
#include "CanTp.h"

/******************************************************************************
*    Construct SF Tx PDU
******************************************************************************/
static uint8 CanTp_Local_ConstructSF(uint8 ChId_u8, uint8 *PduBuffer_au8, PduInfoType *NPduInfo_pst)
{
    CanTp_TxChannelType *l_TxCh_pst;
    const CanTp_TxSduCfg_Type *l_TxSduCfg_cpst;
    PduInfoType l_NSduInfo_st;
    uint8 *l_Data_au8;
    uint8 l_DataFieldOffset_u8;
    uint8 l_PadDataFieldOffset_u8;
    PduLengthType l_RemBufSize;
    uint8 l_RetVal_u8;

    l_TxCh_pst = CanTp_TxChControlBlock + ChId_u8;
    l_TxSduCfg_cpst = CanTp_CfgPtr->TxSdu_cpst + l_TxCh_pst->ActiveSduId_u8;

    /*
     * | N_AE/N_TA | N_PCI | N_Data | N_PadData |
     *                     | <- l_DataFieldOffset_u8
     * */
    l_DataFieldOffset_u8 = CANTP_GET_DATA_FIELD_OFFSET(l_TxSduCfg_cpst->AddrInfo_cst.AddressFormat_u8,
                                                       l_TxCh_pst->NPCI_Type);

    /*
     * | N_AE/N_TA | N_PCI | N_Data | N_PadData |
     *                     | <- l_NSduInfo_st.SduDataPtr
     *                     | XXXXXX | <- l_NPduInfo_st.SduLength
     * */
    l_NSduInfo_st.SduDataPtr = PduBuffer_au8 + l_DataFieldOffset_u8;
    l_NSduInfo_st.SduLength = l_TxCh_pst->TotalSduLength;

    /*
     * | N_AE/N_TA | N_PCI | N_Data | N_PadData |
     * | <- l_NPduInfo_st.SduDataPtr
     * | XXXXXXXXXXXXXXXXXXXXXXXXXX | <- l_NPduInfo_st.SduLength
     * */
    NPduInfo_pst->SduDataPtr = PduBuffer_au8;
    NPduInfo_pst->SduLength = l_DataFieldOffset_u8 + l_NSduInfo_st.SduLength;

    /******************************************************************************
    *    Step: N_Data field
    ******************************************************************************/
	/* TRACE[SWS_CanTp_00086][SWS_CanTp_00272][SWS_CanTp_00298][SWS_CanTp_00299] */
    l_RetVal_u8 = (uint8)PduR_CanTpCopyTxData(l_TxSduCfg_cpst->PduRPduHandleId_u16,
                                                &l_NSduInfo_st,
                                                NULL_PTR,
                                                &l_RemBufSize);

    if(l_RetVal_u8 == (uint8)BUFREQ_OK)
    {
    	/* TRACE[SWS_CanTp_00089] */
        l_Data_au8 = NPduInfo_pst->SduDataPtr;

        /******************************************************************************
        *    Step: N_AE/N_TA field
        ******************************************************************************/
        if(CANTP_GET_ADDRESS_FIELD_SIZE(l_TxSduCfg_cpst->AddrInfo_cst.AddressFormat_u8) != 0)
        {
        	/* TRACE[SWS_CanTp_00281] */
            *l_Data_au8 = l_TxSduCfg_cpst->AddrInfo_cst.Address;
            l_Data_au8++;
        }

        /******************************************************************************
        *    Step: N_Pci field
        ******************************************************************************/
        if(l_TxCh_pst->NPCI_Type == CANTP_NPCI_TYPE_SFCAN)
        {   /* SFCAN */
            l_Data_au8[0] = (uint8)l_TxCh_pst->TotalSduLength;
        }
        else
        {   /* SFCANFD */
            l_Data_au8[0] = 0;
            l_Data_au8[1] = (uint8)l_TxCh_pst->TotalSduLength;
        }
        l_TxCh_pst->RemainingSduLength = l_TxCh_pst->TotalSduLength;

        /******************************************************************************
        *    Step: N_PadData field
        ******************************************************************************/
        /* "10.4.2 CAN frame data padding" of ISO_15765-2 */
        if (((l_TxCh_pst->NPCI_Type == CANTP_NPCI_TYPE_SFCAN) &&
             (l_TxSduCfg_cpst->FlagFields_u8 & CANTP_FLAG_MASK_PADDINGON)) ||
            ((l_TxCh_pst->NPCI_Type == CANTP_NPCI_TYPE_SFCANFD) &&
             (NPduInfo_pst->SduLength < l_TxSduCfg_cpst->TX_DL_u8)))
        {
            /*
             * | N_AE/N_TA | N_PCI | N_Data | N_PadData |
             *                              | <- l_PadDataFieldOffset_u8
             * | XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX | <- l_NPduInfo_st.SduLength
             * */
            l_PadDataFieldOffset_u8 = (uint8)NPduInfo_pst->SduLength;
            NPduInfo_pst->SduLength = CanTp_CanDlTable[NPduInfo_pst->SduLength];	/* TRACE[SWS_CanTp_00348][SWS_CanTp_00351] */

            for (uint16 i = l_PadDataFieldOffset_u8; i < NPduInfo_pst->SduLength; i++)
            {
                l_Data_au8[i] = CANTP_PADDING_BYTE;
            }
        }
    }
    else if(l_RetVal_u8 == (uint8)BUFREQ_E_NOT_OK)
    {
        /* DET: (CANTP_TRIGGERTRANSMIT, CANTP_E_COM) */

    	/* SWS_CanTp_00087 */
    	CanTp_TxChState_au8[ChId_u8] = CANTP_CH_IDLE;
		PduR_CanTpTxConfirmation(l_TxSduCfg_cpst->PduRPduHandleId_u16, E_NOT_OK);
    }
    else
    {
    	/* SWS_CanTp_00184 */
    }

    return l_RetVal_u8;
}

/******************************************************************************
*    Construct FF Tx PDU
******************************************************************************/
uint8 CanTp_Local_ConstructFF(uint8 ChId_u8, uint8 *PduBuffer_au8, PduInfoType *NPduInfo_pst)
{
    CanTp_TxChannelType *l_TxCh_pst;
    const CanTp_TxSduCfg_Type *l_TxSduCfg_cpst;
    PduInfoType l_NSduInfo_st;
    uint32 l_SduLength_u32;
    uint8 *l_Data_au8;
    uint8 l_DataFieldOffset_u8;
    PduLengthType l_RemBufSize;
    uint8 l_RetVal_u8;

    l_TxCh_pst = &CanTp_TxChControlBlock[ChId_u8];
    l_TxSduCfg_cpst = CanTp_CfgPtr->TxSdu_cpst + l_TxCh_pst->ActiveSduId_u8;

    l_TxCh_pst->SN_u8 = 1;
    l_TxCh_pst->RemainingBlockCfsNum_u8 = 0;
    l_TxCh_pst->RemainingSduLength = l_TxCh_pst->TotalSduLength;

    /*
     * Get FF, CF data field offset
     * | N_AE/N_TA | N_PCI | N_Data |
     *                     | <- l_DataFieldOffset_u8
     * */
    l_DataFieldOffset_u8 = CANTP_GET_DATA_FIELD_OFFSET(l_TxSduCfg_cpst->AddrInfo_cst.AddressFormat_u8,
                                                       l_TxCh_pst->NPCI_Type);

    l_TxCh_pst->CFsDataFieldOffset = CANTP_GET_DATA_FIELD_OFFSET(l_TxSduCfg_cpst->AddrInfo_cst.AddressFormat_u8,
                                                            CANTP_NPCI_TYPE_CF);


    /*
     * | N_AE/N_TA | N_PCI | N_Data |
     *                     | <- l_NSduInfo_st.SduDataPtr
     *                     | XXXXXX | <- l_NSduInfo_st.SduLength
     * */
    l_NSduInfo_st.SduDataPtr = PduBuffer_au8 + l_DataFieldOffset_u8;
    l_NSduInfo_st.SduLength = l_TxSduCfg_cpst->TX_DL_u8 - l_DataFieldOffset_u8;

    /*
     * | N_AE/N_TA | N_PCI | N_Data |
     * | <- l_NPduInfo_st.SduDataPtr
     * | TX_DL_u8                      | <- NPduInfo_pst->SduLength
     * */
    NPduInfo_pst->SduDataPtr = PduBuffer_au8;
    NPduInfo_pst->SduLength = l_TxSduCfg_cpst->TX_DL_u8;

    /******************************************************************************
    *    Step: N_Data field
    ******************************************************************************/
	/* TRACE[SWS_CanTp_00086][SWS_CanTp_00272][SWS_CanTp_00298][SWS_CanTp_00299] */
    l_RetVal_u8 = (uint8)PduR_CanTpCopyTxData(l_TxSduCfg_cpst->PduRPduHandleId_u16,
                                                &l_NSduInfo_st,
                                                NULL_PTR,
                                                &l_RemBufSize);

    if(l_RetVal_u8 == (uint8)BUFREQ_OK)
    {
    	/* TRACE[SWS_CanTp_00089] */
        l_Data_au8 = NPduInfo_pst->SduDataPtr;
        l_SduLength_u32 = l_TxCh_pst->TotalSduLength;

        /******************************************************************************
        *    Step: N_AE/N_TA field
        ******************************************************************************/
        if(CANTP_GET_ADDRESS_FIELD_SIZE(l_TxSduCfg_cpst->AddrInfo_cst.AddressFormat_u8) != 0)
        {
        	/* TRACE[SWS_CanTp_00281] */
            *l_Data_au8 = l_TxSduCfg_cpst->AddrInfo_cst.Address;
            l_Data_au8++;
        }

        /******************************************************************************
        *    Step: N_PCI field
        ******************************************************************************/
        if(l_TxCh_pst->NPCI_Type == CANTP_NPCI_TYPE_FFCAN)
        {   /* FFCAN */
            l_Data_au8[0] = 0x10 + (uint8)(l_SduLength_u32 >> 0x08u);
            l_Data_au8[1] = (uint8)(l_SduLength_u32 & 0xFF);
        }
        else
        {   /* FFCANFD */
            l_Data_au8[0] = 0x10;
            l_Data_au8[1] = 0x00;
            l_Data_au8[2] = (uint8)(l_SduLength_u32 >> 24u);
            l_Data_au8[3] = (uint8)(l_SduLength_u32 >> 16u);
            l_Data_au8[4] = (uint8)(l_SduLength_u32 >> 8u);
            l_Data_au8[5] = (uint8)(l_SduLength_u32 & 0xFF);
        }
    }
    else if(l_RetVal_u8 == (uint8)BUFREQ_E_NOT_OK)
    {
    	/* SWS_CanTp_00087 */
    	CanTp_TxChState_au8[ChId_u8] = CANTP_CH_IDLE;
		PduR_CanTpTxConfirmation(l_TxSduCfg_cpst->PduRPduHandleId_u16, E_NOT_OK);
    }
    else
    {
    	/* SWS_CanTp_00184 */
    }

    return l_RetVal_u8;
}

/******************************************************************************
*    Construct CF Tx PDU
******************************************************************************/
static uint8 CanTp_Local_ConstructCF(uint8 ChId_u8, uint8 *PduBuffer_au8, PduInfoType *NPduInfo_pst)
{
    CanTp_TxChannelType *l_TxCh_pst;
    const CanTp_TxSduCfg_Type *l_TxSduCfg_cpst;
    PduInfoType l_NSduInfo_st;
    uint8 *l_Data_au8;
    uint8 l_DataFieldOffset_u8;
    uint8 l_PadDataFieldOffset_u8;
    PduLengthType l_RemBufSize;
    uint8 l_RetVal_u8;

    uint8 l_MaxDataLength_u8;

    l_TxCh_pst = &CanTp_TxChControlBlock[ChId_u8];
    l_TxSduCfg_cpst = CanTp_CfgPtr->TxSdu_cpst + l_TxCh_pst->ActiveSduId_u8;

    /*
     * | N_AE/N_TA | N_PCI | N_Data | N_PadData |
     *                     | <- l_DataFieldOffset_u8
     * */
    l_DataFieldOffset_u8 = l_TxCh_pst->CFsDataFieldOffset;

    l_MaxDataLength_u8 = l_TxSduCfg_cpst->TX_DL_u8 - l_DataFieldOffset_u8;

    /*
     * | N_AE/N_TA | N_PCI | N_Data | N_PadData |
     *                     | <- l_NSduInfo_st.SduDataPtr
     *                     | XXXXXX | <- l_NPduInfo_st.SduLength
     * */
    l_NSduInfo_st.SduDataPtr = PduBuffer_au8 + l_DataFieldOffset_u8;
    l_NSduInfo_st.SduLength = (l_TxCh_pst->RemainingSduLength > l_MaxDataLength_u8) ? l_MaxDataLength_u8 : (uint8)l_TxCh_pst->RemainingSduLength;

    /*
     * | N_AE/N_TA | N_PCI | N_Data | N_PadData |
     * | <- l_NPduInfo_st.SduDataPtr
     * | XXXXXXXXXXXXXXXXXXXXXXXXXX | <- l_NPduInfo_st.SduLength
     * */
    NPduInfo_pst->SduDataPtr = PduBuffer_au8;
    NPduInfo_pst->SduLength = l_DataFieldOffset_u8 + l_NSduInfo_st.SduLength;

    /******************************************************************************
    *    Step: N_Data field
    ******************************************************************************/
	/* TRACE[SWS_CanTp_00086][SWS_CanTP_00272][SWS_CanTp_00298][SWS_CanTp_00299] */
    l_RetVal_u8 = (uint8)PduR_CanTpCopyTxData(l_TxSduCfg_cpst->PduRPduHandleId_u16,
                                              &l_NSduInfo_st,
                                              NULL_PTR,
                                              &l_RemBufSize);

    if (l_RetVal_u8 == (uint8)BUFREQ_OK)
    {
    	/* TRACE[SWS_CanTp_00089] */
        l_Data_au8 = NPduInfo_pst->SduDataPtr;

        /******************************************************************************
        *    Step: N_AE/N_TA field
        ******************************************************************************/
        if (CANTP_GET_ADDRESS_FIELD_SIZE(l_TxSduCfg_cpst->AddrInfo_cst.AddressFormat_u8) != 0)
        {
        	/* TRACE[SWS_CanTp_00281] */
            *l_Data_au8 = l_TxSduCfg_cpst->AddrInfo_cst.Address;
            l_Data_au8++;
        }

        /******************************************************************************
        *    Step: N_PCI field
        ******************************************************************************/
        *l_Data_au8 = (uint8)(0x20 | l_TxCh_pst->SN_u8);

        /******************************************************************************
        *    Step: N_PadData field
        ******************************************************************************/
        /* "10.4.2 CAN frame data padding" of ISO_15765-2 */
        if ((l_NSduInfo_st.SduLength < l_MaxDataLength_u8) &&
            (((l_TxSduCfg_cpst->TX_DL_u8 == CANTP_CAN_DL) && (l_TxSduCfg_cpst->FlagFields_u8 & CANTP_FLAG_MASK_PADDINGON)) ||
             (l_TxSduCfg_cpst->TX_DL_u8 > CANTP_CAN_DL)))
        {
            /*
             * | N_AE/N_TA | N_PCI | N_Data | N_PadData |
             *                              | <- l_PadDataFieldOffset_u8
             * | XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX | <- l_NPduInfo_st.SduLength
             * */
            l_PadDataFieldOffset_u8 = (uint8)NPduInfo_pst->SduLength;
            NPduInfo_pst->SduLength = CanTp_CanDlTable[NPduInfo_pst->SduLength];	/* TRACE[SWS_CanTp_00348][SWS_CanTp_00351] */

            for(uint16 i = l_PadDataFieldOffset_u8; i < NPduInfo_pst->SduLength; i++)
            {
                l_Data_au8[i] = CANTP_PADDING_BYTE;
            }
        }
    }
    else if(l_RetVal_u8 == (uint8)BUFREQ_E_NOT_OK)
    {
    	/* SWS_CanTp_00087 */
    	CanTp_TxChState_au8[ChId_u8] = CANTP_CH_IDLE;
		PduR_CanTpTxConfirmation(l_TxSduCfg_cpst->PduRPduHandleId_u16, E_NOT_OK);
    }
    else
    {
    	/* SWS_CanTp_00184 */
    }

    return l_RetVal_u8;
}

/******************************************************************************
*    Transmit SF/FF/CF NPDU
******************************************************************************/
void CanTp_Local_TxCh_TransmitNPDU(uint8 ChId_u8)
{
    CanTp_TxChannelType *l_TxCh_pst;
    const CanTp_TxSduCfg_Type *l_TxSduCfg_cpst;
    uint8 l_PduBuffer_au8[CANTP_MAX_NPDU_LENGTH];
    PduInfoType l_NPduInfo_st;
    uint8 l_RetVal_u8 = E_OK;
    uint8 l_CurrentTxChState_u8 = CanTp_TxChState_au8[ChId_u8];

    l_TxCh_pst = CanTp_TxChControlBlock + ChId_u8;
    l_TxSduCfg_cpst = CanTp_CfgPtr->TxSdu_cpst + l_TxCh_pst->ActiveSduId_u8;

    /******************************************************************************
    *    Step: Timeout check
    ******************************************************************************/
    CANTP_TIMEROUT_DECREMENT(l_TxCh_pst->StateTimeoutCnt_u16);  /* N_Cs */
    if(l_TxCh_pst->StateTimeoutCnt_u16 == 0)
    {
    	/* TRACE[SWS_CanTp_00280] */
        PduR_CanTpTxConfirmation(l_TxSduCfg_cpst->PduRPduHandleId_u16, E_NOT_OK);
        CanTp_TxChState_au8[ChId_u8] = CANTP_CH_IDLE;
        l_RetVal_u8 = E_NOT_OK;
    }

    if(CANTP_CHECK_TXPDU_UNLOCK(l_TxSduCfg_cpst->TxNPduConirmationPduId_u16) &&
       (l_RetVal_u8 == E_OK))    /* TRACE[SWS_CanTp_00248] */
    {
        /* Construct NPDU */
        switch(l_CurrentTxChState_u8)
        {
            case CANTP_TXCH_TRANSMIT_SF:
                l_RetVal_u8 = CanTp_Local_ConstructSF(ChId_u8, &l_PduBuffer_au8[0], &l_NPduInfo_st);
                break;

            case CANTP_TXCH_TRANSMIT_FF:
                l_TxCh_pst->SeparationTimeCnt_u16 = 0;
                l_RetVal_u8 = CanTp_Local_ConstructFF(ChId_u8, &l_PduBuffer_au8[0], &l_NPduInfo_st);
                break;

            case CANTP_TXCH_TRANSMIT_CF:
                CANTP_TIMEROUT_DECREMENT(l_TxCh_pst->SeparationTimeCnt_u16);
                if(l_TxCh_pst->SeparationTimeCnt_u16 == 0)
                {
                    l_RetVal_u8 = CanTp_Local_ConstructCF(ChId_u8, &l_PduBuffer_au8[0], &l_NPduInfo_st);
                }
                else
                {
                    l_RetVal_u8 = E_NOT_OK;
                }
                break;

            default:
                break;
        }

        if(l_RetVal_u8 == E_OK)
        {
            CANTP_TIMEROUT_SET(l_TxCh_pst->StateTimeoutCnt_u16, l_TxSduCfg_cpst->TimeOut_cst.NaTicks_u16);   /* Set timeout: N_As */
            CanTp_TxChState_au8[ChId_u8] = CANTP_TXCH_WAIT_TRANSMIT_CONFORMATION;
            CANTP_LOCK_TXPDU(l_TxSduCfg_cpst->TxNPduConirmationPduId_u16, ChId_u8);  /* TRACE[SWS_CanTp_00248] */
            /* Transmit NPDU */
            l_RetVal_u8 = CanIf_Transmit(l_TxSduCfg_cpst->CanIfPduHandleId_u16, &l_NPduInfo_st);
            if(l_RetVal_u8 != E_OK)
            {
                CanTp_TxChState_au8[ChId_u8] = CANTP_CH_IDLE;	/* TRACE[SWS_CanTp_00343] */
                CANTP_UNLOCK_TXPDU(l_TxSduCfg_cpst->TxNPduConirmationPduId_u16); /* TRACE[SWS_CanTp_00248] */
            }
        }
    }
}

/******************************************************************************
*    Tx Channel State Process: CANTP_TXCH_WAIT_TRANSMIT_CONFORMATION
******************************************************************************/
void CanTp_Local_TxCh_WaitTransmitConformation(uint8 ChId_u8)
{
    const CanTp_TxSduCfg_Type *l_TxSduCfg_cpst;
    CanTp_TxChannelType *l_TxCh_pst;

    l_TxCh_pst = &CanTp_TxChControlBlock[ChId_u8];
    l_TxSduCfg_cpst = CanTp_CfgPtr->TxSdu_cpst + l_TxCh_pst->ActiveSduId_u8;

    CANTP_TIMEROUT_DECREMENT(l_TxCh_pst->StateTimeoutCnt_u16);
    if(l_TxCh_pst->StateTimeoutCnt_u16 == 0)    /* Check timeout: N_As */
    {
        /* TRACE[SWS_CanTp_00310][SWS_CanTp_00075][SWS_CanTp_00229][SWS_CanTp_00205]
         * In case of N_As timeout Stop connection
         * */
        PduR_CanTpTxConfirmation(l_TxSduCfg_cpst->PduRPduHandleId_u16, E_NOT_OK);
        CANTP_UNLOCK_TXPDU(l_TxSduCfg_cpst->TxNPduConirmationPduId_u16);     /* SWS_CanTp_00248 */
        CanTp_TxChState_au8[ChId_u8] = CANTP_CH_IDLE;
    }
}

/******************************************************************************
*    Tx Channel State Process: CANTP_TXCH_WAIT_FC
******************************************************************************/
void CanTp_Local_TxCh_WaitFC(uint8 ChId_u8)
{
    const CanTp_TxSduCfg_Type *l_TxSduCfg_cpst;
    CanTp_TxChannelType *l_TxCh_pst;

    l_TxCh_pst = &CanTp_TxChControlBlock[ChId_u8];
    l_TxSduCfg_cpst = CanTp_CfgPtr->TxSdu_cpst + l_TxCh_pst->ActiveSduId_u8;

    CANTP_TIMEROUT_DECREMENT(l_TxCh_pst->StateTimeoutCnt_u16);
    if(l_TxCh_pst->StateTimeoutCnt_u16 == 0)    /* Check timeout: N_Bs */
    {
        /* TRACE[SWS_CanTp_00315]
         * TRACE[SWS_CanTp_00316][SWS_CanTp_00229][SWS_CanTp_00205][SWS_CanTp_00205]
         * */
        PduR_CanTpTxConfirmation(l_TxSduCfg_cpst->PduRPduHandleId_u16, E_NOT_OK);
        CanTp_TxChState_au8[ChId_u8] = CANTP_CH_IDLE;
    }
}

void CanTp_Local_TxChannelMain(uint8 ChId_u8)
{
    switch(CanTp_TxChState_au8[ChId_u8])
    {
        case CANTP_TXCH_TRANSMIT_SF:
        case CANTP_TXCH_TRANSMIT_FF:
        case CANTP_TXCH_TRANSMIT_CF:
            CanTp_Local_TxCh_TransmitNPDU(ChId_u8);
            break;

        case CANTP_TXCH_WAIT_TRANSMIT_CONFORMATION:
            CanTp_Local_TxCh_WaitTransmitConformation(ChId_u8);
            break;

        case CANTP_TXCH_WAIT_FC:
            CanTp_Local_TxCh_WaitFC(ChId_u8);
            break;

        default:    /* CANTP_CH_IDLE */
            break;
    }
}

